#ifndef RUNGE_KUTTA_H
#define RUNGE_KUTTA_H
#include <stdio.h>
#include <math.h>
#include "main.h"

#define ACC_correct  1 //启用加速度计修正
#define MAG_correct  0 //启用磁力计修正

typedef struct 
{
    float buff[16];
    uint8_t tail[4];
} vofa_buff_t;

// 定义四元数结构
// 四行一列的矩阵
typedef struct
{
    float q0; // 实部
    float q1; // 虚部 x
    float q2; // 虚部 y
    float q3; // 虚部 z
} Quaternion_t;

/*定义一个三维向量*/
typedef struct
{
    float x;
    float y;
    float z;
} Vector3f_t;

/* 定义一组数据 有3个元素 */
typedef struct
{   
    float x;
    float y;
    float z;

}group3_t;

typedef struct
{
    Quaternion_t quat;   // 四元数
    group3_t gyro; // 角速度
    Vector3f_t acc; // 加速度
    Vector3f_t mag; // 磁场

    group3_t error; // 误差
    group3_t error_integ; // 误差积分
    group3_t error_last; // 上一次误差
    group3_t error_differ; // 误差微分

    group3_t mag_b ;// 磁场修正
    group3_t mag_diff; //磁场微分

    const float diff_max; //积分限幅
    const float diff_mim; //积分限幅

    float kp;       //融合比例
    float ki;       //积分比例
    float kd;       //微分比例

    float roll;
    float pitch;
    float yaw;

} IMU_t;


extern IMU_t imu;

Vector3f_t normalizeacceleration(Vector3f_t vector);
Quaternion_t eulerToQuaternion(float roll, float pitch, float yaw);

Quaternion_t updateQuaternion(Quaternion_t q, float wx, float wy, float wz, float h);

Quaternion_t updateAttitude(Quaternion_t quaternion, float accx,float accy,float accz,float wx, float wy, float wz, float mx,float my,float mz, float time);
#endif // RUNGE_KUTTA_HPP